/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Takes Occu map, a measurement set that is already matched and matches the measuements into the map
* 
*/
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <string>
#include <vector>
#include "owndebug.h"
#include "MatchStorage.h"
#include "EnvironmentMeasurement2D.h"
#include "OccuGUI.h"
#include "BetBdtLocalizer.h"
#include "ownutils.h"
#include "CParticleFilter.h"
#include <cstdlib>   // C standard includes
#include <iostream>  // C++ I/O
#include <string>    // C++ strings
#include "KMlocal.h" // k-means algorithms

#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG

//(21.750000 , 24.379999 , -3.449088) SET1
// ./match2map2 rocla-map.occu map_matched_set1.txt 21.75 24.67 -3.40 map2_matched_set1.txt
///Initial pose
Grid::pose global_ref_pose;
std::string logfilename="scan_match_out.txt";
TOccupancyGrid oplot;

struct TMatchData{
	Grid::pose odo;
	Grid::pose map;
	int ind;
	float weight;
};

// execution parameters (see KMterm.h and KMlocal.h)
KMterm term(1000, 0, 0, 0,               // run for 100 stages
             0.10, 0.10, 3,             // other typical parameter values
             0.50, 10, 0.95);



float timer(bool isStart){
	static long int ss1,us1;
	long int ss2,us2,sec,us,ms;
	float fms;
	
	if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
	else{
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
	}
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/// Read Scan measurement file with arbitrary number of items (and allowing white space before '\n')
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, Grid::pose &p ){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}

void writeScanLine(FILE *f,Grid::TScan &meas, float x,float y,float a){
	fprintf(f,"%f %f %f ",x,y,a); ///< Pose
	int i;
	for(i=0;i<meas.N;i++){
		fprintf(f,"%f %f ",meas.r[i],meas.a[i]);
	}
	fprintf(f,"%f %f\n",meas.r[i],meas.a[i]);
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			v = data[Nx*j+i];
			if(v>0.6) v = 0; else v = 0.99; 
			
			setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
	for(int ii=i-1;ii<i+1;ii++){
		for(int jj=j-1;jj<j+1;jj++){
			setRGBpix(rgb,ii,jj,Nx,r,g,b);
		}
	}
	
}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
	float dy = sin(angle);
	float dx = cos(angle);
	float rr=0;
	int i=0;
	uint x,y;
	while(rr<length){
		x = pix + i*dx;
		y = piy - i*dy;
		setRGBpixBold(RGB,x,y, Nx, r,g,b);
		i++;
		rr+=resolution;
	}
}

void drawLineThin(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
									uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
									unsigned char r,unsigned char g,unsigned char b){
	float dy = sin(angle);
	float dx = cos(angle);
	float rr=0;
	int i=0;
	uint x,y;
	while(rr<length){
		x = pix + i*dx;
		y = piy - i*dy;
		setRGBpix(RGB,x,y, Nx, r,g,b);
		i++;
		rr+=resolution;
	}
}

void drawCross(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							 uint pix, uint piy, float length, ///< Start pixel, angle to draw and linelength
							 unsigned char r,unsigned char g,unsigned char b){
	float angle = M_PI/4;
	for(int i=0;i<3;i++){
		drawLineThin(RGB, Nx,Ny,resolution, pix, piy,angle, length, r,g,b);
		angle+=M_PI/2;
	}
}





void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/// SLAM-MAP Matcher
/// Matches local SLAM maps to the global map
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void globalSLAMMatch(const char *meas_file, 
										CBetBdtLocalizer *glocalizer)
{
		CMatchStorage matches(10000);
		Grid::pose pos;
		std::vector< Grid::pose > poses;    ///All poses for previous map
		std::vector<TPoseMatch> bestOnes;
		
		int enableVisualization = 0;
		FILE *logfilu = fopen(logfilename.c_str(),"wt");
		FILE *fdebug = fopen("debug_scans.txt","wt");
		FILE *log = fopen(meas_file,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		
		FILE *matchout = fopen("matchdataout.txt","wt");
		
		float max_laser_dist=45,maxLaserDistance=max_laser_dist;
		float enough_scan_poinsts=600;
		float edge_th=35.0;
		float grid_size = 90;
		int poseJumpNum = 100; /// How many poses are jumeped between estimates
		
		Grid::TScan meas, first;
		meas.alloc(enough_scan_poinsts);
		first.alloc(enough_scan_poinsts);
		std::vector< std::vector<float> > scans; ///All scans for previous map
		std::vector<float> tmp_scan;
		
		Grid::pose init_est,diff;
		Grid::pose last,current;
		Grid::pose reference_pose, pose_now, global_search;
		
		///Global initial estimate and search area
		//global_ref_pose.set(20,20,0);
		global_search.set(1.0,1.0,M_PI/16.0);
		
		CEnvironmentMeasurement2D global_meas;
		global_meas.frame.setError(global_search.x,global_search.y,global_search.a);		
		glocalizer->tuneParameters(15.0,0.2);
		
		
		int Nx,Ny;
		glocalizer->grid->getDim(Nx,Ny);
		float greso=glocalizer->grid->getResolution();
		
		TOccupancyGrid O(grid_size,grid_size,greso,Grid::TCoord2D(0,0));
		O.setGridToValue(0.5);
		int oNx,oNy;
		O.getDim(oNx,oNy);
		unsigned char *grgb = (unsigned char *) malloc(Nx*Ny*sizeof(unsigned char));
		
		int counter=0;
		scans.clear();
		poses.clear();
		
		///Set the first scan
		readScanLine(log,first,current);
		meas.filt(max_laser_dist);
		
		tmp_scan.clear();
		
		for(int i=0;i<meas.N;i++){
			tmp_scan.push_back(first.r[i]);
		}
		
		scans.push_back(tmp_scan);
		poses.push_back(current);
		
		vector<TMatchData> matchdata;
		
		///READ ALL THE MEASUREMENTS!
		while(!feof(log)){
				readScanLine(log,meas,current);
				meas.filt(max_laser_dist);
				///Save scans and poses 
				tmp_scan.clear();
				
				for(int i=0;i<meas.N;i++){
					if(meas.r[i]>max_laser_dist) meas.r[i]=0;
						tmp_scan.push_back(meas.r[i]);
				}
				scans.push_back(tmp_scan);
				poses.push_back(current);
		}
		
		bool isFirst=true;
		int firstIndex=0;
		init_est.set(0,0,0);
		Grid::pose origin = global_ref_pose;
		int pose_to_estimate=0;
		int measnum=0;
		while(1){
			
			///Copy scan data
			for(int j=0;j<meas.N;j++){
				meas.r[j]=scans[measnum][j];
				meas.a[j]=first.a[j];
			}
			current = poses[measnum];

			if(isFirst){
				fprintf(stderr,"Measnum=%d, Poseto=%d\n",measnum,pose_to_estimate);
					isFirst = false;
					O.setScanFillProb(0, 0 ,0, meas,0.3,0.95); ///This should be the last scan
					last=current;
					firstIndex = measnum;
				}else{
					diff.setToDifferentialPose(current,last);
					init_est = init_est.integrateDifferential(diff);
					O.setScanFillProb(init_est.x,init_est.y,init_est.a, meas,0.3,0.95); ///Fill the measurements into grid
					last=current;
					writeScanLine(fdebug,meas,init_est.x,init_est.y,init_est.a);
					fflush(fdebug);
				}
				measnum++;
				//fprintf(stderr,"(%f, %f, %f)\n", init_est.x,init_est.y,init_est.a);
				////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
				////////////////////////////////////////////////////////////////////////////
				pos = init_est;
				float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
				float toEdge;
				toEdge = (grid_size/2.0)-toPos;			
				
				if(toEdge < edge_th || measnum>=poses.size()){
						fprintf(stderr,"Est: Measnum = %d\n",measnum);
						global_ref_pose = poses[firstIndex];	
						global_meas.setOccupancyGrid(&O, 0.8); ///< set grid to 2D measurement 
						global_meas.frame.setFrame(global_ref_pose.x,global_ref_pose.y,global_ref_pose.a);	
						glocalizer->localize(global_meas,matches);
						matches.sort();
						
						TCovariance c;
						bool ret = glocalizer->computeCovariance(matches.storage[0], global_meas, 15,15,15,0.04,0.04, 0.2*M_PI/180.0,c);
						
					
						
						fprintf(stderr,"Res:: (%.2f,%.2f,%.2f) w1=%.4f M:(G:%d B:%d Fit:%.3f)\n avg:(%lf,%lf,%lf) var:(%lf,%lf,%lf) xy=%lf xa=%lf ya =%lf\n",
										matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,
										matches.storage[0].weight,
										matches.storage[0].StatNumGoodMeas, 
										matches.storage[0].StatNumOutliers, 
										matches.storage[0].StatMeasurementDistSum,
										c.ax,c.ay,c.aa,
										c.cxx,c.cyy,c.caa,
										c.cxy, c.cxa, c.cya);
										
						 ///< this is the matched reference
						global_ref_pose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						
						
						
						///the first scan is missing
						for(int j=0;j<meas.N;j++){
							meas.r[j]=scans[firstIndex][j];
						}
						oplot.setScanFillProb(global_ref_pose.x,global_ref_pose.y,global_ref_pose.a, meas,0.3,0.9);
										
						///Add measurements to the map and save
						int lastInd = firstIndex+poseJumpNum;
						if(lastInd > poses.size()-1) lastInd = poses.size()-1;
						for(unsigned int i=firstIndex;i<lastInd;i++){
								for(int j=0;j<meas.N;j++){
										meas.r[j]=scans[i+1][j];
								}
								diff.setToDifferentialPose(poses[i+1],poses[i]);
								global_ref_pose = global_ref_pose.integrateDifferential(diff);
								oplot.setScanFillProb(global_ref_pose.x,global_ref_pose.y,global_ref_pose.a, meas,0.3,0.9);
								writeScanLine(logfilu,meas, global_ref_pose.x,global_ref_pose.y,global_ref_pose.a);
						}
						
						
						pos.set(0,0,0); ///< initialize pose
						init_est.set(0,0,0);
						O.setGridToValue(0.5);
						
						oplot.fillRGB(grgb, 1.0/255.0);
						
						int oNx,ony;
						oplot.getDim(oNx,oNy);
						
						for(int i=matches.storage.size()-1;i>=0;i--){
							Grid::TPixel pix = oplot.toPixel(Grid::TCoord2D(matches.storage[i].x,matches.storage[i].y));
							float fscale = (matches.storage[i].weight -  matches.storage[0].weight) /
							(matches.storage[matches.storage.size()-1].weight-matches.storage[0].weight);
							float x = fscale;
							
							float r = (x >= 3.0/8.0 & x < 5.0/8.0)*(4. * x - 3./2.)+(x >= 5./8. & x < 7./8.)+(x >= 7./8.) * (-4. * x + 9./2.);
							float g = (x >= 1./8. & x < 3./8.)*(4. * x - 1./2.)+(x >= 3./8. & x < 5./8.)+(x >= 5./8. & x < 7./8.)*(-4. * x + 7./2.);
							float b = (x < 1./8.)*(4. * x + 1./2.)+(x >= 1./8. & x < 3./8.)+(x >= 3./8. & x < 5./8.)*(-4. * x + 5./2.);
							//fprintf(stderr,"%f %f %f \n", r,g,b);
							
							
							int scale = (int) (fscale*255*255*255);
							
							
							//fprintf(stderr,"%f ",scale);
							
							if(x<0.2){
								setRGBpix(grgb,pix.x,pix.y, oNx, (uchar)(b*255),  (uchar)(g*(255)), (uchar) (r*255));
								bestOnes.push_back(matches.storage[i]);
							}
							//setRGBpix(grgb,pix.x,pix.y, oNx, (uchar) (r*255),  (uchar)(g*(255)), (uchar)(b*255));
						}
						
						int         k       = 1;                   // number of centers
						int         dim     = 2;                   // dimension
						int         nPts    = bestOnes.size();                  // number of data points
						KMdata dataPts(dim, nPts);                 // allocate data storage
						KMpointArray pa = dataPts.getPts();
						for(int i=0;i<nPts;i++){
							pa[i][0] = (KMcoord) (bestOnes[i].x);
							pa[i][1] = (KMcoord) (bestOnes[i].y);
						}
						bestOnes.clear();
						dataPts.buildKcTree();                     // build filtering structure
						
						float meandist = 100;
						while(meandist>0.02 && k<10){
							KMfilterCenters ctrs(k, dataPts);           // allocate centers
																													// run the algorithm
							//KMlocalLloyds       kmAlg(ctrs, term);      // repeated Lloyd’s
							// KMlocalSwap      kmAlg(ctrs, term);      // Swap heuristic
							// KMlocalEZ_Hybrid kmAlg(ctrs, term);      // EZ-Hybrid heuristic
							KMlocalHybrid    kmAlg(ctrs, term);      // Hybrid heuristic
							ctrs = kmAlg.execute();      
							meandist = ctrs.getDist(false)/nPts;
							if(meandist<0.02){
								if(k == 1){
									TMatchData m;
									m.odo = poses[pose_to_estimate];
									m.map.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a); 
									m.ind = pose_to_estimate;
									m.weight = meandist;
									
									fprintf(matchout,"%f %f %f %f %f %f %d %f\n",m.odo.x,m.odo.y,m.odo.a,
																											m.map.x,m.map.y,m.map.a,m.ind,m.weight  );
									fflush(matchout);
								}
								// execute
								fprintf(stderr,"Centers = %d Total points = %d\n",k, nPts);
								// print number of stages
								cout << "Number of stages: " << kmAlg.getTotalStages() << "\n";
								// print average distortion
								cout << "Average distortion: " << ctrs.getDist(false)/nPts << "\n";
								ctrs.print();   
								
							}
							else k++;
						}

						///SET TO THE NEXT ESTIMATED POSE
						pose_to_estimate += poseJumpNum;
						measnum = pose_to_estimate;
						firstIndex = measnum;
						isFirst = true;
						
						char buf[50];
						sprintf(buf,"%05d_global-map.jpg",counter);
						saveJPEG(buf, grgb,Nx, Ny);
						if(lastInd == poses.size()-1){
							fprintf(stderr,"***   THE END!!! ***\n");
							fclose(fdebug);
							fclose(logfilu);
							exit(0);
						}
				}
				counter++;
				
				////////////////////////////////////////////////////////////////////////////////
				
		}///While()
		fclose(fdebug);
		fclose(logfilu);
		fclose(matchout);
		
}




/// ./match2map OccuMap MeasurementFile x y a logfilename
int main(int argc, char *argv[]){
		const char *measurement_file_name="../../../data/j2b2_measurement.txt";
		std::string mapfilename;
		int enough_scan_poinsts = 400; ///< enough to reserve memory for the scan points in the meas file.
		
		if(argc!=7){
			fprintf(stderr,"Usage: ./match2map OccuName Measfile X Y A output_logfile \n");
			fprintf(stderr,"X,Y,A is the initial pose\n");
			exit(1);
		}
		mapfilename = argv[1];
		measurement_file_name = argv[2];
		global_ref_pose.x = atof(argv[3]);
		global_ref_pose.y = atof(argv[4]);
		global_ref_pose.a = atof(argv[5]);
		logfilename = argv[6];
		
		
		///Localizers
		CBetBdtLocalizer *glocalizer; 
		glocalizer = new CBetBdtLocalizer(mapfilename.c_str());
		oplot.load(mapfilename.c_str());
		// Init Debug lib
		debugInit();
		debugSetGlobalDebugLvl(1);
		
		//globalSLAMMatch(settings,measurement_file_name,glocalizer,slammer,grgb,srgb);
		globalSLAMMatch(measurement_file_name,glocalizer);
			
		return 0;
}